const int Tripin=12;
const int Echopin=13;
/*JT原创滤波算法：专门针对HC-sr04模块*/
float cm=0;//缓存器
int result=0;//输出结果
int levelLine=0;//滤波水平仪
int i=0;//索引
#define F_Strengh 50//滤波强度：越大精度越小，越延迟；但是越小精度越小，越敏感，一般：50-70
#define Span_Height 5//容错区间：区间越大越迟钝，区间越小越灵敏

int Get_D()//收集原始数据
{
   digitalWrite(Tripin,LOW);
  delayMicroseconds(2);
  digitalWrite(Tripin,HIGH);
  delayMicroseconds(10);
  digitalWrite(Tripin,LOW);
  cm=pulseIn(Echopin,HIGH)/58.0;
  cm=(int(cm*100.0))/100.0;
  if(cm<=0) cm=0;
  return cm;
}

int Getaverage(int * ay)//计算平均数
{
  int sum=0;
  for(i=0;i<F_Strengh;i++)
  {
    sum+=ay[i];
  }
  return (sum/F_Strengh);
}

bool isRange(int a,int b,int num)//判断是否在范围里面
{
    if(num>a&&num<b)
      return true;
     else return false;
}


int ChangeLevelLine()//综合处理
{
    int box[F_Strengh]={0};
    int tp=0;
    for(i=0;i<F_Strengh;i++)
    {
      tp=Get_D();
      if(isRange(levelLine-Span_Height,levelLine+Span_Height,tp))
        return levelLine;
      else
       box[i]=tp;   
    }
    levelLine=Getaverage(box);
    return levelLine;
}

void setup() {
  Serial.begin(9600);
  pinMode(Tripin,OUTPUT);
  pinMode(Echopin,INPUT);
}

void loop() {

  result=ChangeLevelLine();
  Serial.println(result);
}


